#pragma once
#ifndef DRRT_LOCAL_CONNECTOR_H
#define DRRT_LOCAL_CONNECTOR_H

#include "basic_typedef.h"
#include "Prm.h"
#include "MultiRobotCollisionDetector.h"
#include "MultiRobotPRM.h"

struct SearchLevel
{
    double cost;
    double costBack;
    double costNext;
    Point_d level;
    SearchLevel* prev;
    int iterations_left;
} ;

class DRRTLocalConnector
{
public:
    DRRTLocalConnector(vector<Prm*>& prm, MultiRobotCollisionDetector& mr_collision_detector, double eps = 1, int top_values_count = 1, double pessimistic_value = 1);
    ~DRRTLocalConnector();

    list<Point_d> connect(vector<Conf> &p_1, vector<Conf> &p_2, int iterations);

private:
    vector<Prm*>&               m_prm;
    MultiRobotCollisionDetector& m_mr_collision_detector;
    vector<SearchLevel*>        m_visited;
    vector<SearchLevel*>        m_built;

    int                         m_top_values_count;
    double                      m_pessimistic_value;
    double                      m_eps;
    map<Point_d, vector<Conf>, cmpPoint_d>  m_map;      // Reverse maping...

    list<SearchLevel*>           m_possible;
    Point_d                     m_start;
    Point_d                     m_target;

    list<SearchLevel*> generate_top_valid_neighbors(SearchLevel* p);
    list<SearchLevel*> generate_all_valid_neighbors(SearchLevel* p);
    vector<vector<Conf*>> generate_all_directions(SearchLevel* p);
    bool visit(SearchLevel* p);
    SearchLevel* buildItem(Point_d p, SearchLevel* prev, int iterations_left);
    Point_d buildPoint(vector<vector<Conf*>::iterator> &iters);
    list<Point_d> explore();

};

#endif